/* Include files */
#include "Aria.h"
#include <stdio.h>
#include <math.h>
#include <iostream>
using namespace asd;
void wander(double speed, ArRobot *thisRobot);
void trackingWall(ArRobot *robot);

/* Main method */

int main(int argc, char **argv) {
	/* The robot and its devices */
	Aria::init(); //Initialise ARIA library
	ArRobot robot; //Instantiate robot

	ArSonarDevice sonar; //Instantiate its sonar
	robot.addRangeDevice(&sonar); //Add sonar to robot

	ArArgumentParser parser(&argc, argv); //Instantiate argument parser
	ArSimpleConnector connector(&parser); //Instantiate connector
	/* Connection to robot */
	parser.loadDefaultArguments(); //Load default values
	if (!connector.parseArgs()) //Parse connector arguments
	{
		cout << "Unknown settings\n"; //Exit for errors
		Aria::exit(0);
		exit(1);
	}
	if (!connector.connectRobot(&robot)) //Connect to the robot
	{
		cout << "Unable to connect\n"; //Exit for errors
		Aria::exit(0);
		exit(1);
	}
	robot.runAsync(true); //Run in asynchronous mode
	robot.lock(); //Lock robot during set up
	robot.comInt(ArCommands::ENABLE, 1); //Turn on the motors
	robot.unlock(); //Unlock the robot

	trackingWall(&robot);

	robot.waitForRunExit(0);
	Aria::exit(0); //Exit Aria
} //End main
void trackingWall(ArRobot *robot)	{
	double limits;
	limits =1800;
	while(true){

		if((robot->getSonarReading(2))->getRange()>limits && (robot->getSonarReading(3))->getRange()>limits && (robot->getSonarReading(4))->getRange()>limits && (robot->getSonarReading(7))->getSensorTh()==-90){
			cout<<"diritto\n"<<(robot->getSonarReading(7))->getSensorTh();
			robot->setVel2(0,0);
			robot->setVel(300);
			while(robot->isMoveDone()==false){
			   }
		} else {
			cout<<"giro \n";
			//robot.stop();
			robot->setVel2(70,50);
			while(robot->isMoveDone()==false){
			   }
		}

	}

}
